import socket
import threading
import time
from collections import deque
import struct

# 共享数据缓冲区
class SharedBuffer:
    def __init__(self):
        self.buffers = [deque(maxlen=1), deque(maxlen=1)]  # 双缓冲
        self.current = 0
        self.lock = threading.Lock()
    
    def write(self, data):
        with self.lock:
            self.buffers[1 - self.current].append(data)
    
    def read(self):
        with self.lock:
            self.current = 1 - self.current
        return self.buffers[self.current][-1] if self.buffers[self.current] else None

# 初始化缓冲区
listA_buffer = SharedBuffer()
listB_buffer = SharedBuffer()

# UDP配置
UDP_IP_A = "192.168.10.4"  # 需替换实际IP
UDP_PORT_SEND = 10001  # B的发送端口
UDP_PORT_RECV = 10000  # A的发送端口

# 发送线程（B->A）
def sender():
    sock_send = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    while True:
        data = listB_buffer.read()
        if data:
            packed = struct.pack('!I' + 'f'*len(data), len(data), *data)
            sock_send.sendto(packed, (UDP_IP_A, UDP_PORT_SEND))
        time.sleep(0.002)  # 500Hz

# 接收线程（A->B）
def receiver():
    sock_recv = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    sock_recv.bind(("0.0.0.0", UDP_PORT_RECV))
    while True:
        data, _ = sock_recv.recvfrom(1024)
        length = struct.unpack('!I', data[:4])[0]
        listA = struct.unpack('!' + 'f'*length, data[4:])
        listA_buffer.write(listA)

# 启动服务
threading.Thread(target=sender, daemon=True).start()
threading.Thread(target=receiver, daemon=True).start()

# 主循环（500Hz)
now = time.time()
while True:
    start_time = time.perf_counter()
    
    # 生成listB（示例）
    listB = [0,0,0,0,0,0]  # 实际数据生成逻辑
    listB_buffer.write(listB)
    
    # 读取listA（非阻塞）
    current_listA = listA_buffer.read()
    if current_listA:
        print(f"current_listA is {np.array(current_listA)}")
